/*
Autor: Alexander Savochkin, 2010
New BSD License
*/

#pragma once

#include <utility>
#include <string>
#include <boost/lexical_cast.hpp>

#include "IRegulator.h"
#include "Config.h"

using std::string;

using std::pair;
using boost::lexical_cast;

namespace SRCLF 
{

using Config::Configuration;
using Config::Section;
	
namespace Control 
{

	class PIDRegulator : public IRegulator
	{
		double P,I,D;	//PID coeeficients (very clear names in this context :) )
		double error_integral; //Integral in PID formula
		double last_mean_derivative;
		double last_err;
		double desired;
		double regulator_value;
		double exp_moving_average_period;
		ptime last_update;

	public:
		PIDRegulator(const string& name, const Configuration& conf) :
		  P(lexical_cast<double>(conf.getSection(Section::regulator, name).getScalarByKey("P"))), 
		  I(lexical_cast<double>(conf.getSection(Section::regulator, name).getScalarByKey("I"))),
		  D(lexical_cast<double>(conf.getSection(Section::regulator, name).getScalarByKey("D"))),
		  exp_moving_average_period(lexical_cast<double>(conf.getSection(Section::regulator, name).getScalarByKey("T"))),
		  last_update(microsec_clock::universal_time()),error_integral(0.0),last_mean_derivative(0.0),
		  last_err(0.0)
		{}

		void setCoefs(double _P,double _I,double _D) { P = _P; I = _I; D = _D; };

		//Exponential smoothing with rapidity parameter m_dDiffRapidity;
		virtual double diff()
		{
			return 0;
		}

		void update(double input_val, const ptime& new_time);
		double getRegulatorValue();

		void init(double current_input, double current_output, const ptime& current_time);

		virtual void setDesired(double _desired) { desired = _desired; }

		virtual void adminCommand(const string& command, string& result);
		virtual void controlCommand(const string& command, string& result);
		virtual void infoCommand(const string& command, string& result);

	};

} //namespace Control
} //namespace SRCLF

